#ifndef CPU_UTILITY_H
#define CPU_UTILITY_H

namespace poisson
{
	__inline void ConvertStateToFloatVector(const ompl::base::SpaceInformationPtr& si, const ompl::base::State* state, float* pState)
	{
		std::vector<double> dState(7);
		si->getStateSpace()->serialize(&(dState[0]), state);

		for (int i = 0; i < 7; ++i)
			pState[i] = (float)dState[i];

	}
	__inline void ConvertFloatVectorToState(const ompl::base::SpaceInformationPtr& si, const float* pState, ompl::base::State* state)
	{
		std::vector<double> dState(7);

		for (int i = 0; i < 7; ++i)
			dState[i] = (double)pState[i];

		si->getStateSpace()->deserialize(state, &(dState[0]));
	}
}

#endif